#ifndef MM3AWIDGET_H
#define MM3AWIDGET_H



#include <QDebug>
#include <iostream>
#include <fstream>
#include <math.h>
#include <stdio.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include "SerialWidget.h"

#include <QWidget>

#include "opencv/cv.h"
#include <newmat11/newmat.h>

using namespace std;
//using namespace NEWMAT;

/** \brief MM3A widget contains path planning algorithm and function to send commands to the manipulator via the serial port

This Widget contains the planning algorithm that takes in the input velocity from the input device, and decides on the number of steps to given along each joint for the manipulator. The commands are then sent to the manipulator via the serial port.*/

class MM3AWidget : public QWidget
{
    Q_OBJECT

public:
    MM3AWidget(QWidget *parent = 0);
    ~MM3AWidget();


    /** limits for workspace */
    int xlimit[2], ylimit[2];

    Matrix xPix;    /**< x in Pixels */

    void sendCommand(Matrix delta);

    void recordPoints();        // function to record points based on input from the user.

    bool simulate_status; /**< simulate status variable */

    bool expPlane; /**< boolean to set experiment plane. 0 = XY, 1 = XZ */

    Matrix xdGinPix; /**< stores global target position in terms of pixels */

    void variableStepMag(double velMag, double velDir); /**< variable Step Mag method */

    void setEstCorner(int); /**< records estimated position of corner in mm when a corner is reached */

    // getter functions
    int getModelMode();
    int getOperatorMode();


public slots:
    // function to reset mm3a experiment
    void reset();

    // slot to detect change of model
    void setModel(int);

    // slot to detect change of operator mode
    void setOperatorMode(int);

    // slot to detect change in joint update mode
    void setJointUpdateMode(int);

signals:
    // signal to emit value of xPix to display widget
    void xPixels(Matrix pix);

protected:

private:
    /** estimated corner position in pixels */
    Matrix estCorner;

    /** no of points recorded during calibration */
    Matrix recPoints;
    int rCount;

    /** step count for each joint */
    int stepCountA;
    int stepCountB;
    int stepCountC;

    /** defining step size variables */
    float stepSizeA;
    float stepSizeB;
    float stepSizeC;


    Matrix P;         /** P matrix that stores step size for each joint */

    /** defining step size model constants. using naming convention from Aayush's thesis. g1 stands for  gamma 1 followed by values of q3, theta, and psi for gamma 2, and q2, theta, and psi for gamma 3 followed by plus or minus for direction */
    double g1q30t0p0Plus;
    double g1q30t0p0Minus;

    double g2q30t0p0Plus;
    double g2q312t0p0Plus;
    double g2q30t_90p0Plus;
    double g2q312t_90p0Plus;
    double g2q30t0p0Minus;
    double g2q312t0p0Minus;
    double g2q30t_90p0Minus;
    double g2q312t_90p0Minus;

    double g3q20t0p0Plus;
    double g3q2_90t0p0Plus;
    double g3q20t_90p0Plus;
    double g3q20t0p0Minus;
    double g3q2_90t0p0Minus;
    double g3q20t_90p0Minus;

    // model parameters for g1
    double d1Plus;
    double d1q30Plus;
    double d1q312Plus;
    double d1Minus;
    double d1q30Minus;
    double d1q312Minus;

    // model parameters for g2
    double d2Plus;
    double d2q30Plus;
    double d2q312Plus;
    double d2Minus;
    double d2q30Minus;
    double d2q312Minus;

    // model parameters for g3
    double aPlus;
    double aMinus;
    double bPlus;
    double bMinus;
    double cPlus;
    double cMinus;

    /** Model mode. 0: nominal model, 1: true model */
    int modelMode;

    /** Operator mode. 0: blind, 1: redirecting */
    int operatorMode;

    /** Joint update mode. 0-> update joints with estimates, 1-> lock joints */
    int jointUpdateMode;

    Matrix Ji;     /**< impulse jacobian */
    Matrix J;      /**< manipulator jacobian */
    Matrix q;      /**< initial configuration */
    Matrix qf;     /**< final configuration */
    Matrix qf1;
    Matrix xi;      /**< intial position */
    Matrix xf;      /**< final position */

    Matrix xMM;   /**< x in mm */


    Matrix dQ;                       /**< change in configuration found by inverse kinematics */
    Matrix dq;                        /**< change in configuration */
    Matrix dx;                        /**< change in position */
    // Matrix delta;                    /**< matrix of input pulses */
    Matrix xiG;                      /**< global xi for each input from user */
    Matrix xdG;                     /**< global xd for each input from user */
    Matrix zeroX;                  /**< position of end effector at zero position */

    Matrix tVect;                   /**< target vector */

    // model slopes
    double mAaclk;
    double mAclk;
    double c;        // constant
    double L2;      // coefficient for link 2
    double mA;

    double mCin;
    double mCout;
    double cAaclk;
    double cAclk;
    double cCin;
    double cCout;
    double mass;

    double mC;

    Matrix proxy;   /**< Proxy point */

    Matrix center;  /**< origin in image space (pixels) */

    Matrix H;         /**< homography matrix for frame conversion from image space to object space */

    /** file to save data */
    FILE *fp_data;

    // defining DH parameter for the robot in mm and radians. REPLACE WITH MATRICES
    double a1;
    double a2;
    double a3;
    double d1;
    double d2;
    double d3;
    double tool;    /** length of probe tip and holder */
    double toolOffset;
    double theta1;
    double theta2;
    double theta3;
    double alpha1;
    double alpha2;
    double alpha3;
    Matrix a, d, theta, alpha;

    Matrix zero3x3;
    Matrix zero3x1;

    SerialWidget *port; /**< Serial port object */

    Matrix updateMM3A(Matrix q, Matrix delta, int code); /**< updates position of mm3a using impulse Jacobian */

    Matrix mm3aFk(Matrix q); /**< solves the forward kinematics problem for the mm3a */

    Matrix mm3aIk(Matrix x, Matrix y, Matrix q); /**< solves the inverse kinematics problem for the mm3a using the Jacobian */

    Matrix findSteps(Matrix, Matrix, Matrix ); /**< find no of steps along each axis for desired end-effector velocity */

    double calcAngleError(Matrix x); /**< calculates direction error */

    Matrix convertPixelstoMM(Matrix x, int convMode); /**< function to convert pixels to mm */
    // code=1-> pixels to mm
    // code=-1->mm to pixels

    void updateStepSize(Matrix);


};


#endif // MM3AWIDGET_H
